#include "astar_planning_debug_core.h"

AstarPlanning::AstarPlanning():nh_(""), private_nh_("~"){
    private_nh_.param<std::string>("input_map_topic_", input_map_topic_, "");
    private_nh_.param<std::string>("input_start_topic_", input_start_topic_, "");
    private_nh_.param<std::string>("input_goal_topic_", input_goal_topic_, "");
    private_nh_.param<std::string>("output_path_topic_", output_path_topic_, "");
    private_nh_.param<std::string>("output_mask_topic_", output_mask_topic_, "");
    private_nh_.param<std::string>("output_nav_path_topic_", output_nav_path_topic_, "");
    private_nh_.param<double>("inflate", inflate, 0.0);
    private_nh_.param<bool>("Euclidean", Euclidean, true);
    costmap_sub_ = nh_.subscribe<nav_msgs::OccupancyGrid>(input_map_topic_, 1, &AstarPlanning::costmap_callback, this);
    start_sub_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>(input_start_topic_, 1, &AstarPlanning::start_callback, this);
    goal_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>(input_goal_topic_, 1, &AstarPlanning::goal_callback, this);
    mask_costmap_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(output_mask_topic_, 1);
    nav_path_pub_ = nh_.advertise<nav_msgs::Path>(output_nav_path_topic_, 1);
}

AstarPlanning::~AstarPlanning(){}

void AstarPlanning::start_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg){
    ROS_INFO("[%s] subscribe start", __APP_NAME_);
    start_pose_ptr_ = std::make_shared<geometry_msgs::PoseWithCovarianceStamped>(*msg);
    start_ready = true;
}

void AstarPlanning::goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg){
    ROS_INFO("[%s] subscribe goal", __APP_NAME_);
    goal_pose_ptr_ = std::make_shared<geometry_msgs::PoseStamped>(*msg);
    goal_ready = true;
    if(start_ready && costmap_ready){
        int start_index, start_mx, start_my, goal_index, goal_mx, goal_my;
        start_mx = coordinate_2_mxy(start_pose_ptr_->pose.pose.position.x, start_pose_ptr_->pose.pose.position.y)[0];
        start_my = coordinate_2_mxy(start_pose_ptr_->pose.pose.position.x, start_pose_ptr_->pose.pose.position.y)[1];
        start_index = coordinate_2_index(start_pose_ptr_->pose.pose.position.x, start_pose_ptr_->pose.pose.position.y);
        goal_mx = coordinate_2_mxy(goal_pose_ptr_->pose.position.x, goal_pose_ptr_->pose.position.y)[0];
        goal_my = coordinate_2_mxy(goal_pose_ptr_->pose.position.x, goal_pose_ptr_->pose.position.y)[1];
        goal_index = coordinate_2_index(goal_pose_ptr_->pose.position.x, goal_pose_ptr_->pose.position.y);

        if(start_mx<0 || start_mx>cost_map_ptr_->info.width || start_my<0 || start_my>cost_map_ptr_->info.height){
            ROS_WARN("[%s] invalid start, not in map", __APP_NAME_);
            return;
        }

        if(goal_mx<0 || goal_mx>cost_map_ptr_->info.width || goal_my<0 || goal_my>cost_map_ptr_->info.height){
            ROS_WARN("[%s] invalid goal, not in map", __APP_NAME_);
            return;
        }

        if(mask_map_ptr_->data[start_index]>100){
            ROS_WARN("[%s] invalid start, start is in obstacle", __APP_NAME_);
            return;
        }

        if(mask_map_ptr_->data[goal_index]>100){
            ROS_WARN("[%s] invalid goal, goal is in obstacle", __APP_NAME_);
            return;
        }


        AstarNode current_Astar_node;
        AstarNode start_Astar_node;
        AstarNode goal_Astar_node;

        std::vector<AstarNode> open_list;
        std::vector<AstarNode> close_list;

        start_Astar_node.index = start_index;
        start_Astar_node.costG = 0.0;
        start_Astar_node.costH = 0.0;
        start_Astar_node.parentIndex = -1;
        
        goal_Astar_node.index = goal_index;
        goal_Astar_node.costG = 0.0;
        goal_Astar_node.costH = 0.0;
        goal_Astar_node.parentIndex = -1;

        /* 设置起点为当前处理点 */
        current_Astar_node = start_Astar_node;

        /* 将起点加入openlist */
        open_list.push_back(start_Astar_node);
        
        std::vector<int> neighbor_mx_plus = {-1, -1, -1,  0, 0,  1, 1, 1};
        std::vector<int> neighbor_my_plus = {-1,  0,  1, -1, 1, -1, 0, 1};

        while (!is_in_nodeList(goal_index, open_list)){
            current_Astar_node = get_min_f_node(open_list);

            /* openlist中删除该Node */
            for(auto it = open_list.begin(); it != open_list.end(); ++it){
                if(it->index == current_Astar_node.index){
                    open_list.erase(it);
                    break;
                }
            }

            /* closelist中加入该点 */
            close_list.push_back(current_Astar_node);
            
            /* 获取当前点的mx和my */
            int current_mx = index_2_mxy(current_Astar_node.index)[0];
            int current_my = index_2_mxy(current_Astar_node.index)[1];

            /* 对当前的8个临近点进行排查 */
            for(unsigned int i=0; i<=7; i++){
                int neighbor_mx = current_mx + neighbor_mx_plus[i];
                int neighbor_my = current_my + neighbor_my_plus[i];

                /* 越界不做处理 */
                if(neighbor_mx<0 || neighbor_mx>=cost_map_ptr_->info.width || neighbor_my<0 || neighbor_my>=cost_map_ptr_->info.height){
                    continue;
                }

                /* 该点为障碍物不做处理 */
                int neighbor_index = mxy_2_index(neighbor_mx, neighbor_my);
                if(mask_map_ptr_->data[neighbor_index]>100){
                    continue;
                }

                /* 节点已在closelist中不做处理 */
                if(is_in_nodeList(neighbor_index, close_list)){
                    continue;
                }

                /* 如果该节点不在openlist中，计算其G和H代价，并设置父节点索引，放入openlist中 */
                if(!is_in_nodeList(neighbor_index, open_list)){
                    /* G代价用欧式距离 H代价用曼哈顿距离 */
                    double costG, costH;
                    costG = 10.0*hypot(fabs(neighbor_mx-current_mx), fabs(neighbor_my-current_my));
                    if(Euclidean){
                        costH = 10.0*hypot(fabs(neighbor_mx-goal_mx), fabs(neighbor_my-goal_my));
                    }else{
                        costH = 10.0*(fabs(neighbor_mx-goal_mx) + fabs(neighbor_my-goal_my));
                    }
                    /* 生成结点，并放入openlist中 */
                    AstarNode neighbor_astar_node;
                    neighbor_astar_node.index = neighbor_index;
                    neighbor_astar_node.costG = costG;
                    neighbor_astar_node.costH = costH;
                    neighbor_astar_node.parentIndex = current_Astar_node.index;
                    open_list.push_back(neighbor_astar_node);
                }else{
                    /* 判断是否有更小的costG */
                    double new_costG = current_Astar_node.costG + 10.0*hypot(fabs(neighbor_mx-current_mx), fabs(neighbor_my-current_my));
                    AstarNode neighbor_astar_node;
                    for(auto p:open_list){
                        if(p.index==neighbor_index){
                            neighbor_astar_node = p;
                            break;
                        }
                    }

                    if(new_costG<neighbor_astar_node.costG){
                        for(auto p:open_list){
                            if(p.index==neighbor_index){
                                p.costG = new_costG;
                                p.parentIndex = current_Astar_node.index;
                                break;
                            }
                        }
                    }
                }
            }
        }

        /* 获取路径 从目标点找父节点 */
        nav_msgs::Path nav_path;
        nav_path.header.frame_id = mask_map_ptr_->header.frame_id;
        while(current_Astar_node.parentIndex>=0){
            geometry_msgs::PoseStamped pose;
            geometry_msgs::Point point = index_2_coordinate(current_Astar_node.parentIndex);
            pose.header.frame_id = mask_map_ptr_->header.frame_id;
            pose.pose.position.x = point.x;
            pose.pose.position.y = point.y;
            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 1.0;
            nav_path.poses.push_back(pose);
            for(auto p:close_list){
                if(p.index==current_Astar_node.parentIndex){
                    current_Astar_node = p;
                    break;
                }
            }
        }
        reverse(nav_path.poses.begin(), nav_path.poses.end());
        nav_path_pub_.publish(nav_path);
        ROS_INFO("[%s] astar planning successfully", __APP_NAME_);
    }
}

void AstarPlanning::costmap_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg){
    cost_map_ptr_ = std::make_shared<nav_msgs::OccupancyGrid>(*msg);
    nav_msgs::OccupancyGrid mask_map = mask(*cost_map_ptr_);
    mask_map_ptr_ = std::make_shared<nav_msgs::OccupancyGrid>(mask_map);
    costmap_ready = true;
    mask_costmap_pub_.publish(mask_map);
}

nav_msgs::OccupancyGrid AstarPlanning::mask(nav_msgs::OccupancyGrid & costmap){
    nav_msgs::OccupancyGrid mask_map;
    mask_map.header = costmap.header;
    mask_map.info = costmap.info;
    for(unsigned int i=0; i<costmap.data.size(); i++){
        mask_map.data.push_back(0);
    }

    /* 存储占位栅格点索引 */
    std::vector<int> danger_index_vector;
    for(unsigned int i=0; i<costmap.data.size(); i++){
        if(costmap.data[i]>=10){
            danger_index_vector.push_back(i);
        }
    }

    for(unsigned int i=0; i<costmap.data.size(); i++){
        geometry_msgs::Point current_point = index_2_coordinate(i);
        for(unsigned int j=0; j<danger_index_vector.size(); j++){
            geometry_msgs::Point ref_point = index_2_coordinate(danger_index_vector[j]);
            if(hypot(current_point.x-ref_point.x, current_point.y-ref_point.y)<=inflate){
                mask_map.data[i] = 127;
                break;
            }
        }
    }
    return mask_map;
}

int AstarPlanning::coordinate_2_index(double x, double y){
    int mx, my;
    mx = (int)((x-cost_map_ptr_->info.origin.position.x)/cost_map_ptr_->info.resolution);
    my = (int)((y-cost_map_ptr_->info.origin.position.y)/cost_map_ptr_->info.resolution);
    return my*cost_map_ptr_->info.width+mx;
}

geometry_msgs::Point AstarPlanning::index_2_coordinate(int index){
    int mx, my;
    my = index/cost_map_ptr_->info.width;
    mx = index - my*cost_map_ptr_->info.width;
    double x = cost_map_ptr_->info.origin.position.x + mx*cost_map_ptr_->info.resolution;
    double y = cost_map_ptr_->info.origin.position.y + my*cost_map_ptr_->info.resolution;
    geometry_msgs::Point point;
    point.x = x;
    point.y = y;
    return point;
}

std::vector<int> AstarPlanning::coordinate_2_mxy(double x, double y){
    int mx, my;
    mx = (int)((x-cost_map_ptr_->info.origin.position.x)/cost_map_ptr_->info.resolution);
    my = (int)((y-cost_map_ptr_->info.origin.position.y)/cost_map_ptr_->info.resolution);
    std::vector<int> mxy_vector;
    mxy_vector.push_back(mx);
    mxy_vector.push_back(my);
    return mxy_vector;
}

std::vector<int> AstarPlanning::index_2_mxy(int index){
    int mx, my;
    my = index/cost_map_ptr_->info.width;
    mx = index - my*cost_map_ptr_->info.width;
    std::vector<int> mxy_vector;
    mxy_vector.push_back(mx);
    mxy_vector.push_back(my);
    return mxy_vector;
}

int AstarPlanning::mxy_2_index(int mx, int my){
    return my*cost_map_ptr_->info.width + mx;
}

bool AstarPlanning::is_in_nodeList(int index, std::vector<AstarNode>& nodeList){
    for(auto p:nodeList){
        if(p.index==index){
            return true;
        }
    }
    return false;
}

AstarNode AstarPlanning::get_min_f_node(std::vector<AstarNode>& openList){
    AstarNode min_f_node = openList[0];
    for(auto p:openList){
        if((p.costG+p.costH) < (min_f_node.costG + min_f_node.costH)){
            min_f_node = p;
        }
    }
    return min_f_node;
}

